# -*- coding:utf-8 -*-
# @FileName :double_line_change.py
# @Time :2024/5/3 10:21
# @Author :ShengYe
# @Des :
import sys
sys.path.append("/home/dsy/code/python/carla_study")
import carla
import math
from my_code.my_utils import generate_dlc_path
from my_controller.controller import Vehicle_control
from planner import planning_utils



class CarlaWorld:
    def __init__(self, ego_spawn_point_index=7):
        self.client = carla.Client("172.18.0.2", 2000)  # Update to local server if needed
        self.client.set_timeout(10)
        self.world = self.client.load_world('town05')
        self.amap = self.world.get_map()

        All_spawn_points = self.world.get_map().get_spawn_points()
        self.ego_spawn_point = All_spawn_points[ego_spawn_point_index]
        self.ego_vehicle = self.spawn_ego_vehicle()

        # Create cameras for different perspectives

        self.top_camera = self.spawn_camera((0, 10), "top_camera")

        self.fps_camera = self.spawn_camera((0.8, 1.7), "fps_camera")
    def spawn_ego_vehicle(self):
        model3_bp = self.world.get_blueprint_library().find('vehicle.tesla.model3')
        model3_bp.set_attribute('color', '255,88,0')
        model3_actor = self.world.spawn_actor(model3_bp, self.ego_spawn_point)

        print("【车辆已经生成】transform为", self.ego_spawn_point)
        physics_control = carla.VehiclePhysicsControl()
        physics_control.mass = 1412
        model3_actor.apply_physics_control(physics_control)
        return model3_actor

    def spawn_camera(self, location, name):
        camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
        camera_bp.set_attribute('image_size_x', '800')
        camera_bp.set_attribute('image_size_y', '600')
        camera_bp.set_attribute('fov', '110')
        if name == "top_camera":
            camera_transform = carla.Transform(carla.Location(x=location[0], z=location[1]), carla.Rotation(pitch=-90))
        else:
            camera_transform = carla.Transform(carla.Location(x=location[0], z=location[1]))
        camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.ego_vehicle)
        camera.listen(lambda image: self.process_image(name, image))
        return camera

    def process_image(self, camera_name, image):

        array = np.frombuffer(image.raw_data, dtype=np.uint8)
        array = np.reshape(array, (image.height, image.width, 4))
        array = array[:, :, :3]
        if camera_name == "fps_camera":
            self.fps_image = array
        elif camera_name == "top_camera":
            self.top_image = array

    def get_ego_vehicle_info(self):
        ego_vehicle = self.ego_vehicle
        physics_control = ego_vehicle.get_physics_control()
        wheel1 = physics_control.wheels[1].position
        wheel3 = physics_control.wheels[3].position
        dis13 = math.sqrt((wheel1.x - wheel3.x) ** 2 + (wheel1.y - wheel3.y) ** 2)
        wheelbase = dis13 / 100
        lf = wheelbase / 2
        lr = wheelbase / 2
        mass = physics_control.mass
        vehicle_para = (lr, lf, -110000, -110000, mass, 1537)
        return vehicle_para

    def get_controller(self, path):
        return Vehicle_control(ego_vehicle=self.ego_vehicle,
                               vehicle_para=self.get_ego_vehicle_info(),
                               pathway=path,
                               controller_type="MPC_controller")

    def set_spectator(self):
        self.world.get_spectator().set_transform(
            carla.Transform(self.ego_vehicle.get_transform().location +
                            carla.Location(z=50),
                            carla.Rotation(pitch=-90))
        )

    def get_lat_acc(self):
        acc = self.ego_vehicle.get_acceleration()
        acc_vec = np.array([acc.x, acc.y, acc.z])

        right_vec = self.ego_vehicle.get_transform().get_right_vector()
        right_vec = np.array([right_vec.x, right_vec.y, right_vec.z])
        lat_acc = np.dot(acc_vec, right_vec)

        return lat_acc

    def get_velocity(self):
        V = self.ego_vehicle.get_velocity()
        return 3.6 * math.sqrt(V.x * V.x + V.y * V.y + V.z * V.z)

    def get_direction(self):
        return self.ego_vehicle.get_transform().rotation.yaw * (math.pi / 180)

    def get_distance(self, path):
        location = self.ego_vehicle.get_transform().location
        return location.distance(path[-1][0].transform.location)

    def emergence_brake(self):
        brake_control = carla.VehicleControl()
        brake_control.steer = 0
        brake_control.throttle = 0
        brake_control.brake = 1
        self.ego_vehicle.apply_control(brake_control)

    def clean(self):
        if self.ego_vehicle is not None:
            self.ego_vehicle.destroy()
        if self.fps_camera is not None:
            self.fps_camera.stop()
            self.fps_camera.destroy()
        if self.top_camera is not None:
            self.top_camera.stop()
            self.top_camera.destroy()
        settings = self.world.get_settings()
        settings.synchronous_mode = False
        settings.fixed_delta_seconds = None


import pygame
import numpy as np

class PygameDisplay:
    def __init__(self):
        pygame.init()

        self.width, self.height = 1280, 720
        self.screen = pygame.display.set_mode((self.width, self.height))
        pygame.display.set_caption("Carla Visualization")
        self.font = pygame.font.Font(None, 15)
        self.clock = pygame.time.Clock()
        self.fps_image = None
        self.top_image = None

    def update(self, velocity, position, direction):
        # 填充背景
        self.screen.fill((0, 0, 0))

        # 绘制半透明的黑色背景
        black_overlay = pygame.Surface((250, self.height))
        black_overlay.set_alpha(128)  # 设置透明度
        black_overlay.fill((0, 0, 0))
        self.screen.blit(black_overlay, (0, 0))

        # 绘制驾驶员视角图像
        if CARLA.fps_image is not None:
            fps_img_surface = pygame.surfarray.make_surface(np.transpose(CARLA.fps_image, (1, 0, 2)))
            fps_img_surface = pygame.transform.scale(fps_img_surface, (self.width, self.height))  # 全屏显示
            self.screen.blit(fps_img_surface, (0, 0))  # 绘制到画面中间
        else:
            print("FPS image is None")

        # 绘制俯视图像
        if CARLA.top_image is not None:
            top_img_surface = pygame.surfarray.make_surface(np.transpose(CARLA.top_image, (1, 0, 2)))
            top_img_surface = pygame.transform.scale(top_img_surface, (300, 170))  # 调整大小
            top_img_surface.set_alpha(200)  # 设置透明度
            self.screen.blit(top_img_surface, (self.width - 300, 0))  # 绘制到右上角
        else:
            print("Top image is None")

        text_background = pygame.Surface((180, self.height))
        text_background.set_alpha(150)  # 设置透明度
        text_background.fill((0, 0, 0))
        self.screen.blit(text_background, (0, 0))

        # 显示数据信息
        velocity_text = self.font.render(f"Speed: {velocity:.2f} km/h", True, (255, 255, 255))
        position_text = self.font.render(f"Location: ({position[0]:.2f}, {position[1]:.2f})", True, (255, 255, 255))
        direction_text = self.font.render(f"Direction: {direction:.2f} rad", True, (255, 255, 255))

        self.screen.blit(velocity_text, (10, 10))
        self.screen.blit(position_text, (10, 20))
        self.screen.blit(direction_text, (10, 30))

        pygame.display.flip()
        self.clock.tick(30)

    def handle_events(self):
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                exit()



def main():
    try:
        CARLA.set_spectator()
        max_speed = 50

        DISPLAY = PygameDisplay()

        while True:
            CARLA.world.tick()
            CARLA.set_spectator()

            control = Controller.run_step(target_speed=max_speed)
            debug.draw_point(carla.Location(Controller.Lat_control.x_pre, Controller.Lat_control.y_pre, 2),
                             size=0.05, color=carla.Color(0, 255, 255), life_time=0)
            debug.draw_point(carla.Location(Controller.Lat_control.x_pro, Controller.Lat_control.y_pro, 2),
                             size=0.05, color=carla.Color(100, 0, 0), life_time=0)

            CARLA.ego_vehicle.apply_control(control)

            dist = CARLA.get_distance(waypoints)
            velocity = CARLA.get_velocity()
            position = [CARLA.ego_vehicle.get_transform().location.x, CARLA.ego_vehicle.get_transform().location.y]
            direction = CARLA.get_direction()

            DISPLAY.update(velocity, position, direction)
            DISPLAY.handle_events()

            if dist < 2:
                CARLA.emergence_brake()
                print("【车辆到达终点】transform为", CARLA.ego_vehicle.get_transform())
                break


    finally:

        CARLA.clean()


if __name__ == "__main__":
    CARLA = CarlaWorld()

    DISPLAY = PygameDisplay()

    debug = CARLA.world.debug

    waypoints = generate_dlc_path.get_waypoints_2(CARLA.ego_spawn_point)

    global_frenet_path = planning_utils.waypoint_list_2_target_path(waypoints)

    Controller = CARLA.get_controller(global_frenet_path)

    main()
